﻿using System;

namespace GeoSketch2.Core
{
    /// <summary>
    /// A class with geometrical formulas
    /// </summary>
    public static class GeoMath
    {
        #region Distance2D
        /// <summary>
        /// Computes the horizontal distance between 2 points
        /// </summary>
        /// <param name="Point1">The first point</param>        
        /// <param name="Point2">The second point</param>
        public static double Distance2D(this Point2D64D Point1, Point2D64D Point2)
        {
            return Norm2D(Point2-Point1);
        }

        /// <summary>
        /// Computes the horizontal square distance between 2 points
        /// </summary>
        /// <param name="Point1">The first point</param>        
        /// <param name="Point2">The second point</param>
        public static double Distance2DSqr(this Point2D64D Point1, Point2D64D Point2)
        {
            return Norm2DSqr(Point2-Point1);
        }

        /// <summary>
        /// Computes the horizontal norm of a vector
        /// </summary>
        /// <param name="Vector">The vector</param>        
        public static double Norm2D(this Point2D64D Vector)
        {
            return Math.Sqrt(Norm2DSqr(Vector));
        }

        /// <summary>
        /// Computes the horizontal square norm of a vector
        /// </summary>
        /// <param name="Vector">The vector</param>        
        public static double Norm2DSqr(this Point2D64D Vector)
        {
            return Vector.X * Vector.X + Vector.Y * Vector.Y;
        }
        
        /// <summary>
        /// Gives the orthogonal distance between a point and a line
        /// </summary>
        /// <param name="Point">The point</param> 
        /// <param name="LinePoint1">The first line point</param> 
        /// <param name="LinePoint2">The second line point</param> 
        public static double DistancePointLine(this Point2D64D Point, Point2D64D LinePoint1, Point2D64D LinePoint2)
        {
            return 
                Math.Abs(((LinePoint2.X - LinePoint1.X) * (Point.Y - LinePoint1.Y) - (Point.X - LinePoint1.X) * (LinePoint2.Y - LinePoint1.Y))) 
                / Distance2D(LinePoint1, LinePoint2); 
        } 
        #endregion

        #region Distance3D
        /// <summary>
        /// Computes the horizontal distance between two points
        /// </summary>
        /// <param name="Point1">The first point</param>
        /// <param name="Point2">The second point</param>
        public static double Distance3D(this Point3D64D Point1, Point3D64D Point2)
        {
            return Norm3D(Point2+-Point1);
        }

        /// <summary>
        /// Computes the horizontal square distance between two points
        /// </summary>
        /// <param name="Point1">The first point</param>
        /// <param name="Point2">The second point</param>
        public static double Distance3DSqr(this Point3D64D Point1, Point3D64D Point2)
        {
            return Norm3DSqr(Point2+-Point1);
        }
        
        /// <summary>
        /// Computes the horizontal norm of a vector
        /// </summary>
        /// <param name="Vector">The vector</param>        
        public static double Norm3D(this Point3D64D Vector)
        {
            return Math.Sqrt(Norm3DSqr(Vector));
        } 

        /// <summary>
        /// Computes the horizontal square norm of a vector
        /// </summary>
        /// <param name="Vector">The vector</param>        
        public static double Norm3DSqr(this Point3D64D Vector)
        {
            return Vector.X * Vector.X + Vector.Y * Vector.Y + Vector.Z * Vector.Z;
        }
        #endregion

        #region Angle conversion
		/// <summary>
        /// Converts an angle in radians to an angle in degrees
        /// </summary>
        /// <param name="Angle">The angle</param> 
        public static double Radians2Degrees(this double Angle)
        {
            return Angle * 180 / Vars.Constants.PI;
        }

        /// <summary>
        /// Converts an angle in radians to an angle in degrees
        /// </summary>
        /// <param name="Angle">The angle</param> 
        public static double Radians2Degrees(this float Angle)
        {
            return Angle * 180 / Vars.Constants.PI;
        }

        /// <summary>
        /// Converts an angle in degrees to an angle in radians
        /// </summary>
        /// <param name="Angle">The angle</param> 
        public static double Degrees2Radians(this double Angle)
        {
            return Angle * Vars.Constants.PI /180 ;
        }

        /// <summary>
        /// Converts an angle in degrees to an angle in radians
        /// </summary>
        /// <param name="Angle">The angle</param> 
        public static double Degrees2Radians(this float Angle)
        {
            return Angle * Vars.Constants.PI / 180;
        } 
	#endregion

        #region Vector operations
        /// <summary>
        /// Gets the cross product between two vectors
        /// </summary>
        /// <param name="Vector1">The first vector</param>
        /// <param name="Vector2">The second vector</param>
        public static Point3D64D CrossProduct(this Point3D64D Vector1, Point3D64D Vector2)
        {
            return new Point3D64D(
                Vector1.Y * Vector2.Z - Vector1.Z * Vector2.Y,
                Vector1.Z * Vector2.X - Vector1.X * Vector2.Z,
                Vector1.X * Vector2.Y - Vector1.Y * Vector2.X
                );
        }

        /// <summary>
        /// Normalizes a vector
        /// </summary>
        /// <param name="Vector">The vector to normalize</param>
        public static void Normalize(this Point3D64D Vector)
        {
            double norm = Vector.Norm3D();
            Vector.X /= norm;
            Vector.Y /= norm;
            Vector.Z /= norm;
        }

        /// <summary>
        /// Homogenizes a vector
        /// </summary>
        /// <param name="Vector">The vector to homogenize</param>
        public static void Homogenize(this Point3D64D Vector)
        {
            Vector.X /= Vector.Z;
            Vector.Y /= Vector.Z;
            Vector.Z = 1;
        }
        #endregion

        #region Circle calculation
        /// <summary>
        /// Gets a circle from 3 points, X,Y : center, Z : radius of the circle
        /// </summary>
        /// <param name="Point1">The first point</param>
        /// <param name="Point2">The second point</param>
        /// <param name="Point3">The third point</param>
        public static Point3D64D CircleFrom3Points(Point2D64D Point1, Point2D64D Point2, Point2D64D Point3)
        {
            Point3D64D Circle = new Point3D64D();
            Circle.X = -((Point3.X * Point3.X - Point2.X * Point2.X + Point3.Y * Point3.Y - Point2.Y * Point2.Y) / (2 * (Point3.Y - Point2.Y)) -
                (Point2.X * Point2.X - Point1.X * Point1.X + Point2.Y * Point2.Y - Point1.Y * Point1.Y) / (2 * (Point2.Y - Point1.Y))) /
                ((Point2.X - Point1.X) / (Point2.Y - Point1.Y) - (Point3.X - Point2.X) / (Point3.Y - Point2.Y));
            Circle.Y = -(Point2.X - Point1.X) / (Point2.Y - Point1.Y) * Circle.X + (Point2.X * Point2.X - Point1.X * Point1.X + Point2.Y * Point2.Y - Point1.Y * Point1.Y) / (2 * (Point2.Y - Point1.Y));
            Circle.Z = Point1.Distance2D(new Point2D64D(Circle.X, Circle.Y));
            return Circle;
        } 
        #endregion

        #region Centroid
        /// <summary>
        /// Gets the centroid of a set of points
        /// </summary>
        /// <param name="Points">Source points</param>
        public static Point2D64D GetCentroid(this Point2D64D[] Points)
        {
            Point2D64D centroid = new Point2D64D();

            for (int i = 0; i < Points.Length; i++)
            {
                centroid.X += Points[i].X;
                centroid.Y += Points[i].Y;
            }
            centroid.X /= Points.Length;
            centroid.Y /= Points.Length;

            return centroid;
        }  
        #endregion

        #region Dot product
        /// <summary>
        /// Computes the dot product between two vectore
        /// </summary>
        /// <param name="Vector1">The first vector</param> 
        /// <param name="Vector2">The second vector</param> 
        public static double DotProduct(this Point2D64D Vector1, Point2D64D Vector2)
        {
            return Vector1.X * Vector2.X + Vector1.Y * Vector2.Y;
        } 
        #endregion

        #region Point transformation

        #region Rotation
        /// <summary>
        /// Computes the rotation of a point around (0,0)
        /// </summary>
        /// <param name="Point">The point to rotate</param> 
        /// <param name="Angle">The angle of rotation, counter clockwise</param> 
        public static Point2D64D PointRotation(Point2D64D Point, double Angle)
        {
            return PointRotation(Point, Angle, new Point2D64D());
        }

        /// <summary>
        /// Computes the rotation of a point around another point
        /// </summary>
        /// <param name="Point">The point to rotate</param> 
        /// <param name="Angle">The angle of rotation, counter clockwise</param> 
        /// <param name="Center">The center of the rotation</param> 
        public static Point2D64D PointRotation(Point2D64D Point, double Angle, Point2D64D Center)
        {
            return new Point2D64D(
                (Point.X - Center.X) * Math.Cos(Angle) - (Point.Y - Center.Y) * Math.Sin(Angle) + Center.X,
                (Point.X - Center.X) * Math.Sin(Angle) + (Point.Y - Center.Y) * Math.Cos(Angle) + Center.Y
            );
        }  
        #endregion

        #endregion

        #region Geometry intersection
        /// <summary>
        /// Computes the intersection of two lines
        /// </summary>
        /// <param name="Line1">The first line</param> 
        /// <param name="Line2">The second line</param> 
        public static Point2D64D LinesIntersection(this Line2D64D Line1, Line2D64D Line2)
        {
            //http://en.wikipedia.org/wiki/Line-line_intersection
            double d = (Line1.Point1.X - Line1.Point2.X) * (Line2.Point1.Y - Line2.Point2.Y) - (Line1.Point1.Y - Line1.Point2.Y) * (Line2.Point1.X - Line2.Point2.X);
            double a = Line1.Point1.X * Line1.Point2.Y - Line1.Point1.Y * Line1.Point2.X;
            double b = Line2.Point1.X * Line2.Point2.Y - Line2.Point1.Y * Line2.Point2.X;
            return new Point2D64D((a * (Line2.Point1.X - Line2.Point2.X) - b * (Line1.Point1.X - Line1.Point2.X)) / d,
                (a * (Line2.Point1.Y - Line2.Point2.Y) - b * (Line1.Point1.Y - Line1.Point2.Y)) / d);
        }

        /// <summary>
        /// Computes intersection of two circles
        /// </summary>
        /// <param name="Center1">The center of the first circle</param> 
        /// <param name="Radius1">The radius of the first circle</param> 
        /// <param name="Center2">The center of the second circle</param> 
        /// <param name="Radius2">The radius of the second circle</param> 
        /// <param name="ControlPoint">The point used to select which one of the two intersection is the one wanted, the nearest to the point is the intersection wanted</param> 
        public static Point2D64D CirclesIntersection(Point2D64D Center1, double Radius1, Point2D64D Center2, double Radius2, Point2D64D ControlPoint)
        {
            //http://local.wasp.uwa.edu.au/~pbourke/geometry/2circle/
            double d = Center1.Distance2D(Center2);
            double a = (Radius1 * Radius1 - Radius2 * Radius2 + d * d) / (2 * d);
            double h = Math.Sqrt(Radius1 * Radius1 - a * a);
            Point2D64D PerpFoot = Center1 + a * (Center2 - Center1) / d;

            Point2D64D intersection1 = new Point2D64D(PerpFoot.X + h * (Center2.Y - Center1.Y) / d, PerpFoot.Y - h * (Center2.X - Center1.X) / d);
            Point2D64D intersection2 = new Point2D64D(PerpFoot.X - h * (Center2.Y - Center1.Y) / d, PerpFoot.Y + h * (Center2.X - Center1.X) / d);

            if (intersection1.Distance2DSqr(ControlPoint) < intersection2.Distance2DSqr(ControlPoint))
            {
                return intersection1;
            }
            else
            {
                return intersection2;
            }
        } 
        #endregion
    }
}
